package simulador.astarTestcase;

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
import java.util.*;

/**
 * @author csantos
**/
class NodoStar implements Comparable<NodoStar> {
	private double peso;
	private double pesoAteMim;
	private double pesoAteDestino; // peso heuristica
	private NodoStar vimDe;

	public int x, y;
	public NodoStar pai()
	{
		return this.vimDe;
	}
	public NodoStar(int x, int y, double peso, double pesoAteMim, double pesoAteDestino, NodoStar pai)
	{
		this.x = x; this.y = y; this.peso = peso; this.pesoAteMim = pesoAteMim; this.pesoAteDestino = pesoAteDestino; vimDe = pai;
	}
	public double peso()
	{
		return this.peso;
	}
	public double pesoTotal()
	{
		return this.peso + this.pesoAteMim + this.pesoAteDestino;
	}
	public double pesoGarantido()
	{
		return this.peso + this.pesoAteMim;
	}
	public double pesoAteDestino()
	{
		return this.peso + this.pesoAteDestino;
	}
	public double pesoAteMim()
	{
		return this.pesoAteMim;
	}

	public int compareTo(NodoStar other) {
		double difPesos = (this.pesoTotal() - other.pesoTotal());
		if (difPesos > 0.0)
			return 1;
		else if(difPesos == 0.0)
			return 0;
		else
			return -1;
	}
	
	public void ajustaOrigem(NodoStar pai)
	{
		this.vimDe = pai;
		this.pesoAteMim = pai.pesoTotal();
	}
	
	public boolean equals(NodoStar outro)
	{
		return outro.x==this.x && outro.y==this.y;
	}
}

public class AStar {
	private PriorityQueue<NodoStar> piscinaNodos;
	Scenario cenario; // Usado para trabalhar em cima do mapa
	int origX, origY, destX, destY;

	private int distanciaHeuristica(NodoStar orig)
	{
		int dx = Math.abs(orig.x-destX);
		int dy = Math.abs(orig.y-destY);
		if(dx > dy)
			return dx;
		else
			return dy;
	}
	
	private int distanciaHeuristica(int origX, int origY)
	{
		int dx = Math.abs(origX-destX);
		int dy = Math.abs(origY-destY);
		if(dx > dy)
			return dx;
		else
			return dy;
	}
	
	public void setScenario(Scenario cena)
	{
		this.cenario = cena;
	}
	public void setCurrentPos(int x, int y)
	{
		origX = x; origY = y;
	}
	public void setGoal(int x, int y)
	{
		destX = x; destY = y;
	}

	public List<Atom> bestPath()
	{
		ArrayList<Atom> retorno = new ArrayList<Atom>();
		//double cost = scenario.getCostGrid(x, y);
		
		int width = (int)Scenario.getWidth();
		int height = (int)Scenario.getHeight();
		piscinaNodos = new PriorityQueue<NodoStar>();
		
		NodoStar[][] caminho = new NodoStar[height][width];
// Edit 1: comparar tmp com numm no while inverso
// Edit 2: Somar origY com j e origX com i nos loops
// Edit 3: Somar melhorNodo.y com j e melhorNodo.x com i nos loops
// Edit 4: Inicializar piscinaNodos
// Edit 5: remover aux
// Edit 6: ou dentro DOS ifS do j e i != 0
		// Primeiro passo: selecionar o melhor vizinho
		NodoStar melhorNodo;
		for(int j=-1; j <=1; j++) {
			for(int i=-1; i <= 1; i++) {
				if((j!=0 || i!=0) &&
					j+origY >= 0 && j+origY < height &&
					i+origX >= 0 && i+origX < width) {
					System.out.println("novo caminho:" + caminho[origY+j][origX+i]);
					caminho[origY+j][origX+i] = new NodoStar(origX+i, origY+j, Scenario.getCostGrid(origX+i, origY+j), 0, distanciaHeuristica(origX+i, origY+j), caminho[origY][origX]);
					piscinaNodos.add(caminho[origY+j][origX+i]);
				}
			}
		}
		melhorNodo = piscinaNodos.poll();//.peek();
		while(caminho[destY][destX]==null || !melhorNodo.equals(caminho[destY][destX]))
		{
			for(int j=-1; j <=1; j++) {
				for(int i=-1; i <= 1; i++) {
					if((j!=0 || i!=0) &&
						j+melhorNodo.y >= 0 && j+melhorNodo.y < height &&
						i+melhorNodo.x >= 0 && i+melhorNodo.x < width) {
						if(caminho[melhorNodo.y+j][melhorNodo.x+i] == null)
						{
							caminho[melhorNodo.y+j][melhorNodo.x+i] = new NodoStar(melhorNodo.x+i, melhorNodo.y+j, Scenario.getCostGrid(melhorNodo.x+i, melhorNodo.y+j), 0, distanciaHeuristica(melhorNodo.x+i, melhorNodo.y+j), melhorNodo);
							piscinaNodos.add(caminho[melhorNodo.y+j][melhorNodo.x+i]);
						}
						else {
							if(caminho[melhorNodo.y+j][melhorNodo.x+i].pesoAteMim() > melhorNodo.pesoGarantido())
							{
								// Abaixo: Atualiza o elemento e a sua posicao na piscina (provavelmente ele flutuara, ja que seu peso diminuiu)
								piscinaNodos.remove(caminho[melhorNodo.y+j][melhorNodo.x+i]);
								caminho[melhorNodo.y+j][melhorNodo.x+i].ajustaOrigem(melhorNodo);
								piscinaNodos.add(caminho[melhorNodo.y+j][melhorNodo.x+i]);
							}
						}
					}
				}
			}
			melhorNodo = piscinaNodos.poll();//peek();
		}
		NodoStar tmp = caminho[destY][destX];
		// Abaixo: Percorre o caminho inverso para criar a lista
		while(tmp != null)
		{
			retorno.add(0, Scenario.getAtom(tmp.x, tmp.y)); // Adiciona no inicio da lista
			tmp = tmp.pai();
		}
		return retorno;
	}
}

